using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 约束两个实体以保持相对位置和方向。所有六个自由度都同时求解。
    /// </summary>
    public struct Weld : ITwoBodyConstraintDescription<Weld>
    {
        /// <summary>
        /// 在A的局部空间中从主体A到主体B的偏移。
        /// </summary>
        public Vector3 LocalOffset;
        /// <summary>
        /// 身体B在身体A的局部空间中的目标方向。
        /// </summary>
        public Quaternion LocalOrientation;

        /// <summary>
        /// 位置和方向约束的弹性。
        /// </summary>
        public SpringSettings SpringSettings;

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return WeldTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(WeldTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            ConstraintChecker.AssertUnitLength(LocalOrientation, nameof(Weld), nameof(LocalOrientation));
            ConstraintChecker.AssertValid(SpringSettings, nameof(Weld));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<WeldPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.WriteFirst(LocalOffset, ref target.LocalOffset);
            QuaternionWide.WriteFirst(LocalOrientation, ref target.LocalOrientation);
            SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings);
            GetFirst(ref target.SpringSettings.AngularFrequency) = SpringSettings.AngularFrequency;
            GetFirst(ref target.SpringSettings.TwiceDampingRatio) = SpringSettings.TwiceDampingRatio;
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out Weld description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<WeldPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.ReadFirst(source.LocalOffset, out description.LocalOffset);
            QuaternionWide.ReadFirst(source.LocalOrientation, out description.LocalOrientation);
            SpringSettingsWide.ReadFirst(source.SpringSettings, out description.SpringSettings);
        }
    }

    public struct WeldPrestepData
    {
        public Vector3Wide LocalOffset;
        public QuaternionWide LocalOrientation;
        public SpringSettingsWide SpringSettings;
    }

    public struct WeldProjection
    {
        public Vector3Wide Offset;
        public Vector3Wide OffsetBiasVelocity;
        public Vector3Wide OrientationBiasVelocity;
        public Symmetric6x6Wide EffectiveMass;
        public Vector<float> SoftnessImpulseScale;
        public BodyInertias InertiaA;
        public BodyInertias InertiaB;
    }

    public struct WeldAccumulatedImpulses
    {
        public Vector3Wide Orientation;
        public Vector3Wide Offset;
    }

    public struct WeldFunctions : IConstraintFunctions<WeldPrestepData, WeldProjection, WeldAccumulatedImpulses>
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref WeldPrestepData prestep, out WeldProjection projection)
        {
            bodies.GatherPose(ref bodyReferences, count, out var localPositionB, out var orientationA, out var orientationB);
            projection.InertiaA = inertiaA;
            projection.InertiaB = inertiaB;

            // 焊接约束同时处理6个自由度。约束条件包括：
            // localOrientation*OrientationA=OrientationB
            // 位置A+位置偏移*方向A=位置B
            // 速度导数：
            // angularVelocityA=angularVelocityB
            // linearVelocityA+angularVelocityA x(localOffset*OrientationA)=linearVelocityB
            // 请注意,位置约束类似于球窝关节,不同之处在于锚点位于对象B的质心之上。

            // 从上面看,按[线性A、角度A、线性B、角度B]排序的雅克比是：
            // J=[0,i,0,-i]
            // [i,skewSymmetric(localOffset*OrientationA),-i,0]
            // 其中i是3x3单位矩阵。
            // 有效质量=(J*M^-1*JT)^-1,因为J*M^-1*JT是一个6x6矩阵,所以这会有点棘手：
            // J*M^-1*Jt=[Ia^-1+Ib^-1,Ia^-1*转置(skewSymmetric(localOffset*OrientationA))]
            // [skewSymmetric(localOffset*OrientationA)*Ia^-1,mA^-1+Mb^-1+skewSymmetric(localOffset*OrientationA)*Ia^-1*转置(skewSymmetric(localOffset*OrientationA))]
            // 其中Ia^-1和Ib^-1是a和b的逆惯性张量,Ma^-1和Mb^-1是A和B展开到3x3对角线矩阵的逆质量。
            // 值得注意的是,有效质量和它的倒数是对称的,所以我们可以减少反演计算。
            Symmetric3x3Wide.Add(inertiaA.InverseInertiaTensor, inertiaB.InverseInertiaTensor, out var jmjtA);
            QuaternionWide.TransformWithoutOverlap(prestep.LocalOffset, orientationA, out projection.Offset);
            Matrix3x3Wide.CreateCrossProduct(projection.Offset, out var xAB);
            Symmetric3x3Wide.Multiply(inertiaA.InverseInertiaTensor, xAB, out var jmjtB);
            Symmetric3x3Wide.CompleteMatrixSandwichTranspose(xAB, jmjtB, out var jmjtD);
            var diagonalAdd = inertiaA.InverseMass + inertiaB.InverseMass;
            jmjtD.XX += diagonalAdd;
            jmjtD.YY += diagonalAdd;
            jmjtD.ZZ += diagonalAdd;
            Symmetric6x6Wide.Invert(jmjtA, jmjtB, jmjtD, out projection.EffectiveMass);

            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);
            Symmetric6x6Wide.Scale(projection.EffectiveMass, effectiveMassCFMScale, out projection.EffectiveMass);

            // 计算所有6个自由度的当前约束误差。
            // 计算位置误差和偏置速度。计算误差时请注意减法的顺序-我们希望偏置速度抵消分离。
            Vector3Wide.Subtract(localPositionB, projection.Offset, out var positionError);
            QuaternionWide.ConcatenateWithoutOverlap(prestep.LocalOrientation, orientationA, out var targetOrientationB);
            QuaternionWide.Conjugate(targetOrientationB, out var inverseTarget);
            QuaternionWide.ConcatenateWithoutOverlap(inverseTarget, orientationB, out var rotationError);
            QuaternionWide.GetApproximateAxisAngleFromQuaternion(rotationError, out var rotationErrorAxis, out var rotationErrorLength);

            Vector3Wide.Scale(positionError, positionErrorToVelocity, out projection.OffsetBiasVelocity);
            Vector3Wide.Scale(rotationErrorAxis, rotationErrorLength * positionErrorToVelocity, out projection.OrientationBiasVelocity);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref Vector3Wide orientationCSI, ref Vector3Wide offsetCSI)
        {
            // 回想一下雅克比人：
            // J=[0,i,0,-i]
            // [i,skewSymmetric(localOffset*OrientationA),-i,0]
            // 速度变化为：
            // CSI*J*I^-1
            // 线性脉冲A=偏移量CSI
            // 角度脉冲A=方向CSI+世界偏移量x偏移量CSI
            // 线性脉冲B=-偏移CSI
            // 角度脉冲B=-方向CSI
            Vector3Wide.Scale(offsetCSI, projection.InertiaA.InverseMass, out var linearChangeA);
            Vector3Wide.Add(velocityA.Linear, linearChangeA, out velocityA.Linear);

            // 请注意交叉相对于解迭代的顺序。
            // SolveIteration使用JT将速度转换为约束空间速度,而使用J将约束空间转换为世界空间。
            // 元素被转置,转置后的斜对称矩阵被取反。翻转叉积等同于否定。
            Vector3Wide.CrossWithoutOverlap(projection.Offset, offsetCSI, out var offsetWorldImpulse);
            Vector3Wide.Add(offsetWorldImpulse, orientationCSI, out var angularImpulseA);
            Symmetric3x3Wide.TransformWithoutOverlap(angularImpulseA, projection.InertiaA.InverseInertiaTensor, out var angularChangeA);
            Vector3Wide.Add(velocityA.Angular, angularChangeA, out velocityA.Angular);

            Vector3Wide.Scale(offsetCSI, projection.InertiaB.InverseMass, out var negatedLinearChangeB);
            Vector3Wide.Subtract(velocityB.Linear, negatedLinearChangeB, out velocityB.Linear); // 音符减法;雅可比是-i

            Symmetric3x3Wide.TransformWithoutOverlap(orientationCSI, projection.InertiaB.InverseInertiaTensor, out var negatedAngularChangeB);
            Vector3Wide.Subtract(velocityB.Angular, negatedAngularChangeB, out velocityB.Angular); // 音符减法;雅可比是-i
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse)
        {
            ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref accumulatedImpulse.Orientation, ref accumulatedImpulse.Offset);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref WeldProjection projection, ref WeldAccumulatedImpulses accumulatedImpulse)
        {
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            // CSV=V*JT
            Vector3Wide.Subtract(velocityA.Angular, velocityB.Angular, out var orientationCSV);
            Vector3Wide.Subtract(velocityA.Linear, velocityB.Linear, out var offsetCSV);

            Vector3Wide.CrossWithoutOverlap(velocityA.Angular, projection.Offset, out var offsetAngularCSV);
            Vector3Wide.Add(offsetCSV, offsetAngularCSV, out offsetCSV);

            // 注意减法：这是计算biasVelocity-CSV,稍后我们将计算(biasVelocity-CSV)-柔和度。
            Vector3Wide.Subtract(projection.OrientationBiasVelocity, orientationCSV, out orientationCSV);
            Vector3Wide.Subtract(projection.OffsetBiasVelocity, offsetCSV, out offsetCSV);

            Symmetric6x6Wide.TransformWithoutOverlap(orientationCSV, offsetCSV, projection.EffectiveMass, out var orientationCSI, out var offsetCSI);
            Vector3Wide.Scale(accumulatedImpulse.Offset, projection.SoftnessImpulseScale, out var offsetSoftness);
            Vector3Wide.Scale(accumulatedImpulse.Orientation, projection.SoftnessImpulseScale, out var orientationSoftness);
            Vector3Wide.Subtract(offsetCSI, offsetSoftness, out offsetCSI);
            Vector3Wide.Subtract(orientationCSI, orientationSoftness, out orientationCSI);
            Vector3Wide.Add(accumulatedImpulse.Orientation, orientationCSI, out accumulatedImpulse.Orientation);
            Vector3Wide.Add(accumulatedImpulse.Offset, offsetCSI, out accumulatedImpulse.Offset);

            ApplyImpulse(ref velocityA, ref velocityB, ref projection, ref orientationCSI, ref offsetCSI);
        }

    }


    /// <summary>
    /// 处理一组球窝约束的求解迭代。
    /// </summary>
    public class WeldTypeProcessor : TwoBodyTypeProcessor<WeldPrestepData, WeldProjection, WeldAccumulatedImpulses, WeldFunctions>
    {
        public const int BatchTypeId = 31;
    }
}

